#!/usr/bin/env python3    
# -*- coding:UTF-8 -*-

import rospy
import actionlib  # Use the actionlib package for client and server
from std_msgs.msg import String

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped

from move_base_msgs.msg import MoveBaseActionResult


# Define Goal Points and orientations for TurtleBot in a list


def goto(GoalPoints, client):
    # global client
    for TBpose in GoalPoints:
        TBgoal = assign_goal(TBpose)  # For each goal point assign pose
        client.send_goal(TBgoal)
        client.wait_for_result()

    if (client.get_state() == actionlib.GoalStatus.SUCCEEDED):
        rospy.loginfo("success")
    else:
        rospy.loginfo("failed")


def assign_goal(pose):
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]

    return goal_pose


class Move:
    def __init__(self):
        self.q = 0
        # self.pub = rospy.Publisher('navigation_back', String, queue_size=10)
        self.pub_goal = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
        self.pub_navigation = rospy.Publisher('navigation_back', String, queue_size=10)
        rospy.Subscriber("/start_navigation", String, self.move_begin)
        rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.result_callback)
        self.GoalPoints1 = PoseStamped()  # [[(0,  0, 0), (0.0, 0.0, 0.0, 1.0)]]#人前的点
        self.GoalPoints1.header.frame_id = "map" #guest2
        self.GoalPoints1.pose.position.x = 2.9789662586923047
        self.GoalPoints1.pose.position.y = 0.49483933421019993
        self.GoalPoints1.pose.position.z = 0

        self.GoalPoints1.pose.orientation.x = 0
        self.GoalPoints1.pose.orientation.y = 0
        self.GoalPoints1.pose.orientation.z = 0
        self.GoalPoints1.pose.orientation.w = 0.9979212459786554

        self.GoalPoints2 = PoseStamped()  # [[(0,  0, 0), (0.0, 0.0, 0.0, 1.0)]]#人前的点
        self.GoalPoints2.header.frame_id = "map" #guest2
        self.GoalPoints2.pose.position.x = -0.78
        self.GoalPoints2.pose.position.y = 0.3968
        self.GoalPoints2.pose.position.z = 0

        self.GoalPoints2.pose.orientation.x = 0
        self.GoalPoints2.pose.orientation.y = 0
        self.GoalPoints2.pose.orientation.z = 0
        self.GoalPoints2.pose.orientation.w = 0.7045722855524833

        # self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        # self.client.wait_for_server()


        while not rospy.is_shutdown():
            self.listen()

    def move_begin(self, data):
        if data.data == "shopping":
            self.pub_goal.publish(self.GoalPoints1)
            print("go to point 1")
        if data.data == "back" and self.q==1:
            self.pub_goal.publish(self.GoalPoints2)
            print("go to point 2")
            self.q=2


    def listen(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            rate.sleep()

    def result_callback(self,data):
        if data.status.status == 3 and self.q==0:  # SUCCEEDED
            rospy.loginfo("目标1已到达！")
            self.q=1
            self.pub_navigation.publish("navigation1_finish")
        if data.status.status == 3 and self.q==2:  # SUCCEEDED
            rospy.loginfo("目标2已到达！")
            self.q=2
            self.pub_navigation.publish("navigation2_finish")
            


if __name__ == '__main__':
    rospy.init_node('point_navigation_node')
    # Create a SimpleActionClient of a move_base action type and wait for server.
    m = Move()
    m.listen()
